/* 
 * Routines devoted to the calculation of timesteps, predictors and so.
 * Each of them has a parallelized version using OpenMP.
 * 
 */
 
  #include <stdio.h>
  #include <stdlib.h>
  #include <omp.h>
  #include <math.h>

  #include <common.h>
  #include <ffdot.h>
 
  #define NUM_THREADS 4
  #define SMAX (64.000)
  #define SMIN (0.00000038146972)
  #define min(x,y) (x<y ? x : y)
  
  void initialize_timesteps( SIM_DATA *s ) {
  
  
    int j,k;
    
    double F_i[4];
    double FDOT_i[4];
  
    
    s->Time   = 0.0;
    s->nsteps = 0.0;
    //We search for the minimum, so use a large number!
    s->tmin = 1e10;
    
    //Initialize time steps
    for( j=0; j< s->N; j++ ) {
      
     FFDOT(j, F_i, FDOT_i, s );
      
      for( k=0; k<3; k++ )
      {
        s->F[4*j+k]    = 0.5*F_i[k];
        s->FDOT[4*j+k] = FDOT_i[k]/6.0;
      }
      
      for( k=0; k<4; k++ )
      {
        s->X_p[4*j+k] = s->X[4*j+k];
        s->XDOT_p[4*j+k] = s->XDOT[4*j+k];
      }
      
      s->STEP[j] = sqrt(s->eta*1./sqrt(F_i[0]*F_i[0]+F_i[1]*F_i[1]+F_i[2]*F_i[2]));
      
      //God damn K is an INT, not float!!! T.T
      int K = 2 + (log(SMAX) - log(s->STEP[j]))/log(2.0);
      
      s->STEP[j] = SMAX/pow(2,K-1);
      
      s->tmin = min(s->STEP[j], s-> tmin);
      
      s->T0[j] = 0.0;
    }
  
  }
  
  int build_blocksteps( SIM_DATA *s ) {
    
    int j,l = 0;
    for( j=0; j< s->N; j++ )
    {
      s->NEXT[j] = 0;
      
      //Advance this particle
      if( s->T0[j] + s->STEP[j] == s->tmin )
      {   
        s->NEXT[j] = 1;
        l++;
      }
    }
    
    s->Time = s->tmin;
    

    return l;
  }
  
  void predict( SIM_DATA *s ) {
    
    //Predict all coordinates to order FDOT
    int j,k;  
    for( j=0; j< s->N; j++ )
    {
      double ds = s->Time - s->T0[j];
      for( k=0; k<3; k++)
      {   
           s->X[4*j+k] = ( ( s->FDOT[4*j+k]*ds + s->F[4*j+k] )*ds  + 
                             s->XDOT_p[4*j+k] )*ds + s->X_p[4*j+k];
      
        s->XDOT[4*j+k] = ( s->FDOT[4*j+k]*1.5*ds + s->F[4*j+k] )*2.0*ds + 
                           s->XDOT_p[4*j+k];
      }
    }
    
  }
  
  void advance_block( SIM_DATA *s, int block_size ) {
  
    double F_i[3], FDOT_i[3];
    s->tmin = 1.0e10;
    
    // Advance the next block of particles
    int i,k,tid;
    
    //Keep all possible tmins in a separate array
    double tmin[s->N];
    
    for( i=0; i<s->N; i++ )
		tmin[i] = 1e10;
	
	
	
	
	omp_set_num_threads( min(block_size, omp_get_num_threads() ) );
	
    #pragma omp parallel for private(i,k,tid,F_i,FDOT_i)
    for( i=0; i< s->N; i++ )
    {
		tid = omp_get_thread_num();
		
	    if( s->NEXT[i] == 1)
	    {
		  FFDOT( i, F_i, FDOT_i, s );
		  //Build the Hermite 4th order corrector
	      double dt = s->Time - s->T0[i];
	      s->T0[i] = s->Time;
	      
	      for( k=0; k<3; k++ )
	      {
	        // Some cryptic stuff from the Hermite scheme 
	        double df  = 2.0*s->F[4*i+k] - F_i[k];
	        double fid = 6.0*s->FDOT[4*i+k];
	        double sum = fid + FDOT_i[k];
	        
	        double at3 =  2.0*df + dt*(sum      );
	        double bt2 = -3.0*df - dt*(sum + fid);
	        
	        // Corrector step
	        s->X_p[4*i+k] = (0.6*at3 + bt2)*dt*dt/12.0 + s->X[4*i+k];
	        s->XDOT_p[4*i+k] = (0.75*at3 + bt2)*dt/3.0 + s->XDOT[4*i+k];     
	        
	        s->F[4*i+k]    = 0.5*F_i[k];
	        s->FDOT[4*i+k] = FDOT_i[k]/6.0;
	        
	        // New timestep array
	        s->D2[4*i+k] = (3.0*at3 + bt2)*2.0/(dt*dt);
	        s->D3[4*i+k] = at3*6.0/(dt*dt*dt);  
	      }
	      
	      // New timestep calculation
	      double fi2  = F_i[0]*F_i[0] + F_i[1]*F_i[1] + 
	                    F_i[2]*F_i[2];
	      double fd2  = FDOT_i[0]*FDOT_i[0] + FDOT_i[1]*FDOT_i[1] + 
	                    FDOT_i[2]*FDOT_i[2];
	      double fd22 = s->D2[4*i+0]*s->D2[4*i+0] + s->D2[4*i+1]*s->D2[4*i+1] +
	                    s->D2[4*i+2]*s->D2[4*i+2];
	      double fd32 = s->D3[4*i+0]*s->D3[4*i+0] + s->D3[4*i+1]*s->D3[4*i+1] +
	                    s->D3[4*i+2]*s->D3[4*i+2];
	      
	      // Timestep parameter
	      double si = sqrt(s->eta*(sqrt(fi2*fd22) + fd2)/(sqrt(fd2*fd32) + fd22) );
	      
	      if( si > 2.0*s->STEP[i] && fmod(s->Time,2.0*s->STEP[i]) == 0) {
	        s->STEP[i] = min(2.0*s->STEP[i],SMAX);
	      }
	      
	      if( s->STEP[i] < SMIN ) {
	        s->STEP[i] = SMIN;
	      }
	      
	      if( si < s->STEP[i] ) {
	        s->STEP[i] *= 0.5;
	      }
	      
	      tmin[i] = s->T0[i]+s->STEP[i];
		}
      }
      
      s->nsteps += block_size;
      
      for( i=0; i<s->N; i++ )
      {
		  s->tmin = min(tmin[i],s->tmin);
	  }
      
  }
  
